#pragma once
#include <glm/glm.hpp>
#include <cmath>

namespace glm {

template<typename T>
GLM_FUNC_DECL void jacobiRotate(glm::tmat3x3<T> &A, glm::tmat3x3<T> &R, int p, int q)
{
    // rotates A through phi in pq-plane to set A(p,q) = 0
    // rotation stored in R whose columns are eigenvectors of A
    if (A[p][q] == 0.0)
        return;

    T d = (A[p][p] - A[q][q]) / (static_cast<T>(2.0)*A[p][q]);
    T t = static_cast<T>(1.0) / (fabs(d) + sqrt(d*d + static_cast<T>(1.0)));
    if (d < 0.0) t = -t;
    T c = static_cast<T>(1.0) / sqrt(t*t + static_cast<T>(1));
    T s = t * c;
    A[p][p] += t * A[p][q];
    A[q][q] -= t * A[p][q];
    A[p][q] = 0.0;
    A[q][p] = 0.0;
    // transform A
    int k;
    for (k = 0; k < 3; k++) {
        if (k != p && k != q) {
            T Akp = c * A[k][p] + s * A[k][q];
            T Akq = -s * A[k][p] + c * A[k][q];
            A[k][p] = Akp;
            A[p][k] = Akp;
            A[k][q] = Akq;
            A[q][k] = Akq;
        }
    }
    // store rotation in R
    for (k = 0; k < 3; k++) {
        T Rkp = c * R[k][p] + s * R[k][q];
        T Rkq = -s * R[k][p] + c * R[k][q];
        R[k][p] = Rkp;
        R[k][q] = Rkq;
    }
}

template<typename T>
GLM_FUNC_DECL void eigenDecomposition(glm::tmat3x3<T> &A, glm::tmat3x3<T> &eigenVecs, glm::tvec3<T> &eigenVals)
{
    const int numJacobiIterations = 10;
    const T epsilon = static_cast<T>(1e-15);

    glm::tmat3x3<T> D = A;

    // only for symmetric matrices!
    eigenVecs = glm::tmat3x3<T>(1.0);	// unit matrix
    int iter = 0;
    while (iter < numJacobiIterations) {	// 3 off diagonal elements
                                            // find off diagonal element with maximum modulus
        int p, q;
        T a, max;
        max = fabs(D[0][1]);
        p = 0; q = 1;
        a = fabs(D[0][2]);
        if (a > max) { p = 0; q = 2; max = a; }
        a = fabs(D[1][2]);
        if (a > max) { p = 1; q = 2; max = a; }
        // all small enough -> done
        if (max < epsilon) break;
        // rotate matrix with respect to that element
        jacobiRotate(D, eigenVecs, p, q);
        iter++;
    }
    eigenVals[0] = D[0][0];
    eigenVals[1] = D[1][1];
    eigenVals[2] = D[2][2];
}

template<typename T>
GLM_FUNC_DECL void polarDecomposition(const glm::tmat3x3<T> &A, glm::tmat3x3<T> &R, glm::tmat3x3<T> &S)
{
    // A = SR, where S is symmetric and R is orthonormal
    // -> S = (A A^T)^(1/2)
    // A = U D U^T R

    glm::tmat3x3<T> AAT;
    AAT[0][0] = A[0][0] * A[0][0] + A[0][1] * A[0][1] + A[0][2] * A[0][2];
    AAT[1][1] = A[1][0] * A[1][0] + A[1][1] * A[1][1] + A[1][2] * A[1][2];
    AAT[2][2] = A[2][0] * A[2][0] + A[2][1] * A[2][1] + A[2][2] * A[2][2];

    AAT[0][1] = A[0][0] * A[1][0] + A[0][1] * A[1][1] + A[0][2] * A[1][2];
    AAT[0][2] = A[0][0] * A[2][0] + A[0][1] * A[2][1] + A[0][2] * A[2][2];
    AAT[1][2] = A[1][0] * A[2][0] + A[1][1] * A[2][1] + A[1][2] * A[2][2];

    AAT[1][0] = AAT[0][1];
    AAT[2][0] = AAT[0][2];
    AAT[2][1] = AAT[1][2];

    glm::tmat3x3<T> U;
    glm::tvec3<T> eigenVals;
    eigenDecomposition(AAT, U, eigenVals);

    T d0 = sqrt(eigenVals[0]);
    T d1 = sqrt(eigenVals[1]);
    T d2 = sqrt(eigenVals[2]);
    glm::tmat3x3<T> D(0.0);
    D[0][0] = d0;
    D[1][1] = d1;
    D[2][2] = d2;

    const T eps = static_cast<T>(1e-15);

    T l0 = eigenVals[0]; if (l0 <= eps) l0 = 0.0f; else l0 = 1.0f / d0;
    T l1 = eigenVals[1]; if (l1 <= eps) l1 = 0.0f; else l1 = 1.0f / d1;
    T l2 = eigenVals[2]; if (l2 <= eps) l2 = 0.0f; else l2 = 1.0f / d2;

    glm::tmat3x3<T> S1;
    S1[0][0] = l0 * U[0][0] * U[0][0] + l1 * U[0][1] * U[0][1] + l2 * U[0][2] * U[0][2];
    S1[1][1] = l0 * U[1][0] * U[1][0] + l1 * U[1][1] * U[1][1] + l2 * U[1][2] * U[1][2];
    S1[2][2] = l0 * U[2][0] * U[2][0] + l1 * U[2][1] * U[2][1] + l2 * U[2][2] * U[2][2];

    S1[0][1] = l0 * U[0][0] * U[1][0] + l1 * U[0][1] * U[1][1] + l2 * U[0][2] * U[1][2];
    S1[0][2] = l0 * U[0][0] * U[2][0] + l1 * U[0][1] * U[2][1] + l2 * U[0][2] * U[2][2];
    S1[1][2] = l0 * U[1][0] * U[2][0] + l1 * U[1][1] * U[2][1] + l2 * U[1][2] * U[2][2];

    S1[1][0] = S1[0][1];
    S1[2][0] = S1[0][2];
    S1[2][1] = S1[1][2];

    R = A * S1;

    // stabilize
    glm::tvec3<T> c0 = R[0], c1 = R[1], c2 = R[2];

    if (c0.length() < eps)
        c0 = glm::cross(c1, c2);
    else if (c1.length() < eps)
        c1 = glm::cross(c2, c0);
    else
        c2 = glm::cross(c0, c1);
    R[0] = c0;
    R[1] = c1;
    R[2] = c2;

    S = A * transpose(R);
}

template<typename T>
GLM_FUNC_DECL T trace(glm::tmat3x3<T> &A)
{
    return A[0][0] + A[1][1] + A[2][2];
}

}